﻿import rospy
import os
import numpy as np
from geometry_msgs.msg import PoseStamped


def laneline_publisher():

    allX, allY = np.loadtxt(os.path.dirname( 
        __file__) + './trace.txt', delimiter=' ', usecols=(0, 1), unpack=True)
    rospy.init_node('laneline_publisher', anonymous=True)
    laneline_info_pub = rospy.Publisher(
        '/laneline_info', PoseStamped, queue_size=10)

    rate = rospy.Rate(10)
    index = -1
    while not rospy.is_shutdown():
        index = index + 1
        if index == len(allX):
            index = 0
        target_pose = PoseStamped()
        target_pose.header.seq = 0
        target_pose.header.stamp = rospy.Time.now()
        target_pose.header.frame_id = "map"

        target_pose.pose.position.x = allX[index]
        target_pose.pose.position.y = allY[index]
        target_pose.pose.position.z = 0.0
        target_pose.pose.orientation.x = 0.0
        target_pose.pose.orientation.y = 0.0
        target_pose.pose.orientation.w = 1.0

        laneline_info_pub.publish(target_pose)
        rospy.loginfo("Publish target_pose message[%.6f, %.6f]",
                      target_pose.pose.position.x, target_pose.pose.position.y)
        rate.sleep()


if __name__ == '__main__':
    try:
        laneline_publisher()
    except rospy.ROSInterruptException:
        pass
